package main;

import java.util.ArrayList;

import javax.vecmath.AxisAngle4f;
import javax.vecmath.Color3f;
import javax.vecmath.Point3d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;

import rigidbody.BoxRigidBody;
import rigidbody.ObjModelRigidBody;
import rigidbody.RigidBody;
import rigidbody.SphereRigidBody;
import simulation.Simulator;
import util.Util;
import collision.OBBTree;
import force.GravityForce;
import force.SimpleForce;
import geometry.ObjModel;
import geometry.Triangle;

public class TestScenes
{
    public static void initMassSphereTestScene(Simulator simulator_)
    {
        for (int i = 0; i < 19; i++)
        {
            for (int j = 0; j < 19; j++)
            {
                SphereRigidBody sphereBody = new SphereRigidBody(0.2,
                        new Point3d(1 * (i - 9), 1 * (j - 9), 0.0), 0.4);
                simulator_.addRigidBody(sphereBody);
                simulator_.addTempForce(new SimpleForce(sphereBody,
                        new Point3d(0, 0, 0), new Vector3d(0, 0,
                                (i * 19 + j) % 2 == 1 ? -50 : 50)));
            }
            
        }
    }

    public static void initSingleBoxTestScene(Simulator simulator_)
    {
        RigidBody boxBody = new BoxRigidBody(0.2, new Point3d(0, 0, 0),
                new Vector3d(3, 3, 3));
        simulator_.addRigidBody(boxBody);

        SimpleForce force1 = new SimpleForce(boxBody, new Point3d(-1, 0, 0),
                new Vector3d(0, 20, 0));
        SimpleForce force2 = new SimpleForce(boxBody, new Point3d(0, 1, 0),
                new Vector3d(0, 0, -20));
        ArrayList<RigidBody> boxBodies = new ArrayList<RigidBody>();
        boxBodies.add(boxBody);
        GravityForce force3 = new GravityForce(boxBodies);
        simulator_.addTempForce(force1);
        simulator_.addTempForce(force2);
        simulator_.addPersistantForce(force3);
    }
    
    public static void initTwoBoxCollisionTestScene(Simulator simulator_)
    {
        RigidBody boxBody1 = new BoxRigidBody(1, new Point3d(0, 0.7, 0), new Vector3d(1, 1, 1));
        
        Quat4d q1 = new Quat4d(Math.sin(22.5 / 180 * Math.PI), 0, 0, Math.cos(22.5 / 180 * Math.PI));
        q1.normalize();
        double tmp = Math.sqrt(2) / 2;
        double ang = Math.atan(Math.sqrt(2));
        Quat4d q2 = new Quat4d(0, tmp * Math.sin(ang / 2), -tmp * Math.sin(ang / 2), Math.cos(ang / 2));
        q2.normalize();
        q1.mul(q2);
        q1.normalize();
        
//        Quat4d p = new Quat4d();
//        p.x = 1;
//        p.y = 1;
//        p.z = 1;
//        p.mulInverse(q1);
//        p.mul(q1, p);
        
        boxBody1.getOrientation().mul(q1);
        RigidBody boxBody2 = new BoxRigidBody(1, new Point3d(3, 0, 0), new Vector3d(1, 1, 1));
        simulator_.addRigidBody(boxBody1);
        simulator_.addRigidBody(boxBody2);
        
        SimpleForce force1 = new SimpleForce(boxBody1, new Point3d(0, 0.7, 0), new Vector3d(60, 0, 0));
//        SimpleForce force2 = new SimpleForce(boxBody1, new Point3d(1, 0, 0), new Vector3d(0, 0, 1));
        simulator_.addTempForce(force1);
//        simulator_.addTempForce(force2);
    }
    
    public static void initBoxWithAngryBirdCollisionTestScene(Simulator simulator_)
    {
        RigidBody boxBody1 = new BoxRigidBody(0.2, new Point3d(0, 0, 0), new Vector3d(0.9, 0.9, 0.9));
        
        ObjModel bird = new ObjModel("src/main/AngryBird1", "src/main/AngryBird1.mtl");
        RigidBody objBody = new ObjModelRigidBody(0.2, new Point3d(3, 0, 0), bird);
        simulator_.addRigidBody(boxBody1);
        simulator_.addRigidBody(objBody);
        
        SimpleForce force1 = new SimpleForce(boxBody1, new Point3d(0, 0.1, 0), new Vector3d(20, 0, 0));
        simulator_.addTempForce(force1);
    }
    
    public static void initOverlapBoxes(Simulator simulator_)
    {
    	RigidBody boxBody1 = new BoxRigidBody(0.2, new Point3d(0, 0, 0), new Vector3d(0.9, 0.9, 0.9));
        RigidBody boxBody2 = new BoxRigidBody(0.2, new Point3d(0.5, 0, 0), new Vector3d(1, 1, 1));
        simulator_.addRigidBody(boxBody1);
        simulator_.addRigidBody(boxBody2);
    }
    
    public static void initBoxWithObjModelBoxCollisionTestScene(Simulator simulator_)
    {
        RigidBody boxBody1 = new BoxRigidBody(0.2, new Point3d(0, 0, 0), new Vector3d(0.9, 0.9, 0.9));
        
        ObjModel cube = new ObjModel("src/main/cube");
//        RigidBody boxBody2 = new ObjModelRigidBody(0.2, new Point3d(3, 0, 0), cube);
        simulator_.addRigidBody(boxBody1);
//        simulator_.addRigidBody(boxBody2);
        
        SimpleForce force1 = new SimpleForce(boxBody1, new Point3d(0, 0.1, 0), new Vector3d(20, 0, 0));
        simulator_.addTempForce(force1);
    }
    
    public static void initRestingContact(Simulator simulator_)
    {
    	RigidBody boxBody1 = new BoxRigidBody(10, new Point3d(2, 0.9, 0), new Vector3d(2, 2, 2));
    	RigidBody table = new BoxRigidBody(10, new Point3d(0, -1, 0), new Vector3d(10, 2, 10));
    	
    	double angle = 100.0;
    	Quat4d q1 = new Quat4d(0, 0, Math.sin(angle / 180 * Math.PI), Math.cos(angle / 180 * Math.PI));
        q1.normalize();
//        table.getOrientation().mul(q1);
    	table.setIsImmobile(true);
        simulator_.addRigidBody(boxBody1);
        simulator_.addRigidBody(table);

        ArrayList<RigidBody> boxBodies = new ArrayList<RigidBody>();
        boxBodies.add(boxBody1);
        GravityForce gravity = new GravityForce(boxBodies);
        simulator_.addPersistantForce(gravity);
        
//        SimpleForce force1 = new SimpleForce(boxBody, new Point3d(0, 2, 0), new Vector3d(20, 0, 0));
//        simulator_.addTempForce(force1);
    }
    
    public static void initBoxInBoxes(Simulator simulator_)
    {
    	RigidBody wall1 = new BoxRigidBody(0.2, new Point3d(0, 0, -20), new Vector3d(10, 10, 10));
    	wall1.setIsImmobile(true);
    	simulator_.addRigidBody(wall1);
    	RigidBody wall2 = new BoxRigidBody(0.2, new Point3d(-10, 0, -10), new Vector3d(10, 10, 10));
    	wall2.setIsImmobile(true);
    	simulator_.addRigidBody(wall2);
    	RigidBody wall3 = new BoxRigidBody(0.2, new Point3d(10, 0, -10), new Vector3d(10, 10, 10));
    	wall3.setIsImmobile(true);
    	simulator_.addRigidBody(wall3);
    	RigidBody wall4 = new BoxRigidBody(0.2, new Point3d(0, -10, -10), new Vector3d(10, 10, 10));
    	wall4.setIsImmobile(true);
    	simulator_.addRigidBody(wall4);
    	RigidBody wall5 = new BoxRigidBody(0.2, new Point3d(0, 10, -10), new Vector3d(10, 10, 10));
    	wall5.setIsImmobile(true);
    	simulator_.addRigidBody(wall5);
    	for (int i = 0; i < 3; i++)
    		for (int j = 0; j < 3; j++)
    		{
		    	RigidBody boxBody = new BoxRigidBody(
		    			0.02, new Point3d(i * 3 - 3, j * 3 - 3, -10), new Vector3d(1, 1, 1), 
		    			new Color3f(0.6f, 0.1f, 0.1f));
				SimpleForce force = new SimpleForce(
						boxBody, new Point3d(i * 3 - 3, j * 3 - 2, -10),
						new Vector3d(20 * Math.random(), -10 * Math.random(), -10 * Math.random()));
				simulator_.addRigidBody(boxBody);
				simulator_.addTempForce(force);
    		}
    }
    
    public static void initCrash(Simulator simulator_)
    {
    	RigidBody block = new BoxRigidBody(0.5, new Point3d(2, 0, 3), new Vector3d(0.5, 0.5, 0.5));
    	RigidBody b0 = new BoxRigidBody(0.2, new Point3d(2, 1, -2), new Vector3d(1, 1, 1));
    	RigidBody b1 = new BoxRigidBody(0.2, new Point3d(2.5, 0, -2), new Vector3d(1, 1, 1));
    	RigidBody b2 = new BoxRigidBody(0.2, new Point3d(1.5, 0, -2), new Vector3d(1, 1, 1));
    	RigidBody b3 = new BoxRigidBody(0.2, new Point3d(1, -1, -2), new Vector3d(1, 1, 1));
    	RigidBody b4 = new BoxRigidBody(0.2, new Point3d(2, -1, -2), new Vector3d(1, 1, 1));
    	RigidBody b5 = new BoxRigidBody(0.2, new Point3d(3, -1, -2), new Vector3d(1, 1, 1));
    	SimpleForce force = new SimpleForce(block, new Point3d(2, 0, 3), new Vector3d(0, 0, -200));
    	simulator_.addRigidBody(block);
    	simulator_.addTempForce(force);
    	simulator_.addRigidBody(b0);
    	simulator_.addRigidBody(b1);
    	simulator_.addRigidBody(b2);
    	simulator_.addRigidBody(b3);
    	simulator_.addRigidBody(b4);
    	simulator_.addRigidBody(b5);
    }
    
    public static void initSimpleCrash(Simulator simulator_)
    {
    	RigidBody block = new BoxRigidBody(0.5, new Point3d(3, 0, -1.25), new Vector3d(0.5, 0.5, 0.5));
    	RigidBody b1 = new BoxRigidBody(0.2, new Point3d(3.5, 0, -2), new Vector3d(1, 1, 1));
    	RigidBody b2 = new BoxRigidBody(0.2, new Point3d(2.5, 0, -2), new Vector3d(1, 1, 1));
    	SimpleForce force = new SimpleForce(block, new Point3d(3, 0, 3), new Vector3d(0, 0, -200));
    	simulator_.addRigidBody(block);
    	simulator_.addTempForce(force);
    	simulator_.addRigidBody(b1);
    	simulator_.addRigidBody(b2);
    }
    
    public static void initAngryBirdCrash(Simulator simulator_)
    {
    	ObjModel bird = new ObjModel("src/main/AngryBird1", "src/main/AngryBird1.mtl", 1, null, new AxisAngle4f(0,1,0, 3.5f));
        RigidBody objBody = new ObjModelRigidBody(0.2, new Point3d(-3, 0, -5), bird);
    	RigidBody b0 = new BoxRigidBody(0.2, new Point3d(2, 1, -5), new Vector3d(1, 1, 1));
    	RigidBody b1 = new BoxRigidBody(0.2, new Point3d(2, 0, -4.5), new Vector3d(1, 1, 1));
    	RigidBody b2 = new BoxRigidBody(0.2, new Point3d(2, 0, -5.5), new Vector3d(1, 1, 1));
    	RigidBody b3 = new BoxRigidBody(0.2, new Point3d(2, -1, -6), new Vector3d(1, 1, 1));
    	RigidBody b4 = new BoxRigidBody(0.2, new Point3d(2, -1, -5), new Vector3d(1, 1, 1));
    	RigidBody b5 = new BoxRigidBody(0.2, new Point3d(2, -1, -4), new Vector3d(1, 1, 1));
    	SimpleForce force = new SimpleForce(objBody, new Point3d(-3, 0, -5), new Vector3d(100, 0, 0));
    	simulator_.addRigidBody(objBody);
    	simulator_.addTempForce(force);
    	simulator_.addRigidBody(b0);
    	simulator_.addRigidBody(b1);
    	simulator_.addRigidBody(b2);
    	simulator_.addRigidBody(b3);
    	simulator_.addRigidBody(b4);
    	simulator_.addRigidBody(b5);
    }
    
    public static void initThreeBoxCollisionTestScene(Simulator simulator_)
    {
	    RigidBody boxBody1 = new BoxRigidBody(0.2, new Point3d(1, 0, 0), new Vector3d(0.5, 0.5, 0.5));
	    RigidBody boxBody2 = new BoxRigidBody(0.2, new Point3d(2, 0, 0), new Vector3d(1, 1, 1));
	    RigidBody boxBody3 = new BoxRigidBody(0.2, new Point3d(5, 0, 0), new Vector3d(2, 2, 2));
	    boxBody3.setIsImmobile(true);
	    simulator_.addRigidBody(boxBody1);
	    simulator_.addRigidBody(boxBody2);
	    simulator_.addRigidBody(boxBody3);
	    
	    SimpleForce force1 = new SimpleForce(boxBody1, new Point3d(0, 0, 0), new Vector3d(5, 0, 0));
	    simulator_.addTempForce(force1);
    }
    
    public static void initTwoSphereCollisionTestScene(Simulator simulator_)
    {
        RigidBody sphereBody1 = new SphereRigidBody(0.2, new Point3d(0, 0.1, 0), 1);
        RigidBody sphereBody2 = new SphereRigidBody(0.2, new Point3d(3, 0, 0), 1);
        simulator_.addRigidBody(sphereBody1);
        simulator_.addRigidBody(sphereBody2);
        
        SimpleForce force1 = new SimpleForce(sphereBody1, new Point3d(0, 0, 0), new Vector3d(20, 0, 0));
        simulator_.addTempForce(force1);
    }
    
    public static void initSingleBoxTestScene2(Simulator simulator_)
    {
        RigidBody boxBody = new BoxRigidBody(0.2, new Point3d(0, 0, 0),
                new Vector3d(3, 3, 3));
        simulator_.addRigidBody(boxBody);

        SimpleForce force1 = new SimpleForce(boxBody, new Point3d(-1, 0, 0),
                new Vector3d(0, 20, 0));
        SimpleForce force2 = new SimpleForce(boxBody, new Point3d(0, 1, 0),
                new Vector3d(0, 0, -20));
        ArrayList<RigidBody> boxBodies = new ArrayList<RigidBody>();
        boxBodies.add(boxBody);
        simulator_.addTempForce(force1);
        simulator_.addTempForce(force2);
    }

	public static void initObjectsOnTable(Simulator simulator)
	{
		double box1size = 1.5;
		double box2size = 1.3;
		double box3size = 1.1;
		double box4size = 0.9;
		
		RigidBody boxBody1 = new BoxRigidBody(10, new Point3d(0, 0, 0), new Vector3d(box1size, box1size, box1size));
		RigidBody boxBody2 = new BoxRigidBody(10, new Point3d(0, 2, 0), new Vector3d(box2size, box2size, box2size));
		RigidBody boxBody3 = new BoxRigidBody(10, new Point3d(0, 4, 0), new Vector3d(box3size, box3size, box3size));
		RigidBody boxBody4 = new BoxRigidBody(10, new Point3d(0, 6, 0), new Vector3d(box4size, box4size, box4size));
    	RigidBody table = new BoxRigidBody(0.2, new Point3d(0, -3, 0), new Vector3d(10, 1, 10));
    	table.setIsImmobile(true);
        simulator.addRigidBody(boxBody1);
        simulator.addRigidBody(boxBody2);
        simulator.addRigidBody(boxBody3);
        simulator.addRigidBody(boxBody4);
        simulator.addRigidBody(table);

        ArrayList<RigidBody> boxBodies = new ArrayList<RigidBody>();
        boxBodies.add(boxBody1);
        boxBodies.add(boxBody2);
        boxBodies.add(boxBody3);
        boxBodies.add(boxBody4);
        GravityForce gravity = new GravityForce(boxBodies);
        simulator.addPersistantForce(gravity);
	}

	public static void initObjectsDropping(Simulator simulator)
	{
		double boxsize = 1;
		ArrayList<RigidBody> boxBodies = new ArrayList<RigidBody>();
//		ObjModel bird = new ObjModel("src/main/AngryBird1", "src/main/AngryBird1.mtl", 1, null, new AxisAngle4f(0,1,0, 3.5f));
		ObjModel cone = new ObjModel("src/main/cone.obj", "src/main/cone.mtl", 1, null, null);
		ObjModel monkey = new ObjModel("src/main/monkey.obj", "src/main/monkey.mtl", 1, null, null);
		ObjModel monkey2 = new ObjModel("src/main/monkey.obj", "src/main/monkey.mtl", 0.8f, null, null);
		
		for (int i = 0; i < 20; i++)
		{
//			RigidBody boxBody = new BoxRigidBody(10, new Point3d(0, i - 2, 0), new Vector3d(boxsize - 0.1 / (i + 1), boxsize - 0.1 / (i + 1), boxsize - 0.1 / (i + 1)));
//			boxBodies.add(boxBody);
//			simulator.addRigidBody(boxBody);
//			RigidBody birdBody = new ObjModelRigidBody(10, new Point3d(0, i * 2, 0), bird);
//			boxBodies.add(birdBody);
//			simulator.addRigidBody(birdBody);
//			RigidBody coneBody = new ObjModelRigidBody(10, new Point3d(0, i * 2, 0), cone);
//			boxBodies.add(coneBody);
//			simulator.addRigidBody(coneBody);
			RigidBody monkeyBody = new ObjModelRigidBody(10, new Point3d(0, i * 2, 0), monkey);
			boxBodies.add(monkeyBody);
			simulator.addRigidBody(monkeyBody);
		}
    	RigidBody wall1 = new BoxRigidBody(0.2, new Point3d(0, -3, 0), new Vector3d(10, 1, 10));
    	wall1.setIsImmobile(true);
        simulator.addRigidBody(wall1);
        
        RigidBody wall2 = new BoxRigidBody(0.2, new Point3d(-5, 0, 0), new Vector3d(1, 10, 10));
    	wall2.setIsImmobile(true);
        simulator.addRigidBody(wall2);
        
        RigidBody wall3 = new BoxRigidBody(0.2, new Point3d(5, 0, 0), new Vector3d(1, 10, 10));
    	wall3.setIsImmobile(true);
        simulator.addRigidBody(wall3);
        
        RigidBody wall4 = new BoxRigidBody(0.2, new Point3d(0, 0, -5), new Vector3d(10, 10, 1));
    	wall4.setIsImmobile(true);
        simulator.addRigidBody(wall4);
        
        RigidBody wall5 = new BoxRigidBody(0.2, new Point3d(0, 0, 5), new Vector3d(10, 10, 1));
    	wall5.setIsImmobile(true);
        simulator.addRigidBody(wall5);

        GravityForce gravity = new GravityForce(boxBodies);
        simulator.addPersistantForce(gravity);
	}
	
	public static void initPiles(Simulator simulator)
	{
		RigidBody ground = new BoxRigidBody(0.2, new Point3d(0, -0.5, 0), new Vector3d(10, 1, 10));
    	ground.setIsImmobile(true);
        simulator.addRigidBody(ground);
        ArrayList<RigidBody> boxBodies = new ArrayList<RigidBody>();
        
        double boxSize = 1;
        int levelCount = 3;
        for (int i = levelCount; i >= 0; i--)
        {
        	for (int j = 0; j < i; j++)
        	{
        		double x, y;
        		if (i % 2 == 0)
        			x = j - (i / 2) + 0.5;
        		else
        			x = j - (i / 2);
        		y = boxSize * (levelCount + 0.5 - i);
        		RigidBody boxBody = new BoxRigidBody(1, new Point3d(x, y, 0),
        					new Vector3d(boxSize - y / 10, boxSize, boxSize - y / 10));
        		boxBodies.add(boxBody);
        		simulator.addRigidBody(boxBody);
        	}
        }
        
        GravityForce gravity = new GravityForce(boxBodies);
        simulator.addPersistantForce(gravity);
	}

	public static void initTwoMonkeys(Simulator simulator)
	{
		ArrayList<RigidBody> bodies = new ArrayList<RigidBody>();
//		RigidBody boxBody = new BoxRigidBody(10, new Point3d(0, 0, 0), new Vector3d(1, 1, 1));
		ObjModel monkey = new ObjModel("src/main/monkey.obj", "src/main/monkey.mtl", 1, null, null);
		RigidBody monkeyBody = new ObjModelRigidBody(10, new Point3d(0, 0, 0), monkey);
		simulator.addRigidBody(monkeyBody);
		Point3d point = new Point3d(0.1, 1.1, 0.6);
		SphereRigidBody sphereBody = new SphereRigidBody(1, point, 0.01);
		simulator.addRigidBody(sphereBody);
		OBBTree tree = new OBBTree(monkeyBody.getGeometry());
		double distance = tree.getSignedDistance(point);
		
		Triangle.ClosestPointInfo info = tree.getClosestPoint(point);
		
		assert(distance == Util.distance(info.targetPoint, point)
			|| distance == -Util.distance(info.targetPoint, point));
		
		SphereRigidBody sphereBody2 = new SphereRigidBody(1, info.targetPoint, 0.01, new Color3f(0, 1, 1));
		simulator.addRigidBody(sphereBody2);
		
//		for (int i = 0; i < 1; i++)
//		{
//			ObjModel monkey = new ObjModel("src/main/monkey.obj", "src/main/monkey.mtl", 1, null, null);
//			RigidBody monkeyBody = new ObjModelRigidBody(10, new Point3d(0, i * 1.8, 0), monkey);
//			simulator.addRigidBody(monkeyBody);
//			
//			bodies.add(monkeyBody);
//		}
		
		SimpleForce force1 = new SimpleForce(sphereBody, new Point3d(0, 0, 0), new Vector3d(0, -5, 0));
		simulator.addTempForce(force1);
		
//		simulator.setStaticScene(true);
	}
	
	public static void initMonkeyDropOnTable(Simulator simulator)
	{
		ArrayList<RigidBody> monkeyBodies = new ArrayList<RigidBody>();
		ObjModel monkey = new ObjModel("src/main/monkey.obj", "src/main/monkey.mtl", 1, null, null);
		
		for (int i = 0; i < 1; i++)
		{
			RigidBody monkeyBody = new ObjModelRigidBody(10, new Point3d(0, i * 2, 0), monkey);
			monkeyBodies.add(monkeyBody);
			simulator.addRigidBody(monkeyBody);
		}
    	RigidBody table = new BoxRigidBody(0.2, new Point3d(0, -3, 0), new Vector3d(10, 1, 10));
    	table.setIsImmobile(true);
        simulator.addRigidBody(table);
        
        GravityForce gravity = new GravityForce(monkeyBodies);
        simulator.addPersistantForce(gravity);
	}
	
	public static void initLShapeDropOnTable(Simulator simulator)
	{
		ArrayList<RigidBody> bodies = new ArrayList<RigidBody>();
		ObjModel shape = new ObjModel("src/main/lshape.obj", "src/main/lshape.mtl", 1, null, null);
		
		for (int i = 0; i < 1; i++)
		{
			RigidBody shapeBody = new ObjModelRigidBody(10, new Point3d(0, i * 5 + 5, 0), shape);
			bodies.add(shapeBody);
			simulator.addRigidBody(shapeBody);
		}
    	RigidBody table = new BoxRigidBody(0.2, new Point3d(0, -3, 0), new Vector3d(10, 1, 10));
    	table.setIsImmobile(true);
        simulator.addRigidBody(table);
        
        GravityForce gravity = new GravityForce(bodies);
        simulator.addPersistantForce(gravity);
	}
}
